#include "geometry.h"
#include <algorithm>
#include <cmath>
#include <limits>
#include <gmpxx.h>

namespace
{
   int x_compare(const void * a1, const void * b1)
   {
      point a = *(point*)a1;
      point b = *(point*)b1;

      if (a.x == b.x)
      {
         return 0;
      }

      if (a.x < b.x)
      {
         return -1;
      }

      return 1;
   }

   int y_compare(const void * a1, const void * b1)
   {
      point a = *(point*)a1;
      point b = *(point*)b1;

      if (a.y == b.y)
      {
         return 0;
      }

      if (a.y < b.y)
      {
         return -1;
      }

      return 1;
   }

   bool is_nearest(const point &a, const point &b, const point &c) 
   {
      double f1 = (b.x - a.x) * (b.x - a.x);
      double f2 = (b.y - a.y) * (b.y - a.y);
      double f3 = (c.x - a.x) * (c.x - a.x);
      double f4 = (c.y - a.y) * (c.y - a.y);
      double sub = f1 + f2 - f3 - f4;
      double sumabs = f1 + f2 + f3 + f4;

      if (sub > 10 * sumabs * std::numeric_limits<double>::epsilon()) 
      {
          return true;
      }

      if (sub < - 10 * sumabs * std::numeric_limits<double>::epsilon()) 
      {
        return false;
      }

      mpf_class x1(a.x), y1(a.y), x2(b.x), y2(b.y), x3(c.x), y3(c.y);
      mpf_class subt = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) - (x3 - x1) * (x3 - x1) - (y3 - y1) * (y3 - y1);

      if (subt < 0) 
      {
        return false;
      }
      return true;
   }

}

kd2tree::kd2tree(const std::vector<point> & points)
{
   nodes.resize(points.size() * 4 + 1);

   point * arr = new point[points.size()];
   int n = points.size();
   for (int i = 0;  i < n; i++)
   {
      arr[i] = points[i];
   }

   make_tree(arr, n, 1, true);
   delete[] arr;
}

void kd2tree::make_tree(point* arr, int arr_size, int pos, bool compare)
{
   if (arr_size == 0)
   {
      return;
   }

   if (arr_size == 1)
   {
      nodes[pos] = node(arr[0], compare);
      return;
   }

   if (compare)
   {
      std::qsort(arr, arr_size, sizeof(point), x_compare);
   } else
   {
      std::qsort(arr, arr_size, sizeof(point), y_compare);
   }

   nodes[pos] = node(arr[arr_size / 2], compare);
   int n1 = arr_size / 2;
   point * arr1 = new point[n1];
   for (int i = 0; i < n1; i++)
   {
      arr1[i] = arr[i];
   }
   make_tree(arr1, n1, 2 * pos, !compare);
   delete[] arr1;

   int n2 = arr_size - n1 - 1;
   point * arr2 = new point[n2];
   for (int i = n1 + 1; i < arr_size; i++)
   {
      arr2[i - n1 - 1] = arr[i];
   }
   make_tree(arr2, n2, 2 * pos + 1, !compare);
   delete[] arr2;
}

point kd2tree::find_nearest(point a)
{
   return nodes[get_nearest(a, 1, 1)].p;
}

int kd2tree::get_nearest(point a, int begin, int nearest)
{
   if (!nodes[begin].exist)
   {
      return nearest;
   }

   if (is_nearest(a, nodes[nearest].p, nodes[begin].p))
   {
      nearest = begin;
   }

   int left = begin * 2;
   int right = left + 1;
   point bound;
   if (nodes[begin].orientation)
   {
      if (a.x >= nodes[begin].p.x)
      {
         std::swap(left, right);
      }
      bound = point(nodes[begin].p.x, a.y);
   } else
   {
      if (a.y >= nodes[begin].p.y)
      {
         std::swap(left, right);
      }
      bound = point(a.x, nodes[begin].p.y);
   }

   int cur_nearest = get_nearest(a, left, nearest);
   if (!is_nearest(a, nodes[cur_nearest].p, nodes[nearest].p))
   {
      nearest = cur_nearest;
   }

   if (is_nearest(a, nodes[nearest].p, bound))
   {
      cur_nearest = get_nearest(a, right, nearest);
      if (!is_nearest(a, nodes[cur_nearest].p, nodes[nearest].p))
      {
         nearest = cur_nearest;
      }
   }

   return nearest;
}